#include "robot.h"

#include <QDebug>

Robot::Robot(QString description, QObject* parent) :
    QObject(parent),
    _rootComponent(NULL),
    _description(description),
    _batchSem(0),
    _changed(false)
{}

Robot::~Robot(){
    //Call delete on the root component to recursively
    //delete all of the components of the robot, because
    //each component's destructor deletes its children
    if (_rootComponent != NULL) {
        delete _rootComponent;
    }
}

QString Robot::description() const{
    return _description;
}

bool Robot::isNull() const {
    return false;
}

const RobotComponent* Robot::getComponent(const NameGroup names) const{
    if (_rootComponent != NULL) {
        ComponentFinder finder(names);
        _rootComponent->accept(&finder);

        return finder.component();
    } else {
        return NULL;
    }
}

const QList<int> Robot::jointIDs() const{
    if(_rootComponent != NULL){
        //Use a RobotJointVisitor to get information
        //about the joints
        JointVisitor visitor;
        _rootComponent->accept(&visitor);

        return visitor.map()->keys();
    }
    else{
        return QList<int>();
    }
}

RobotJointGroup* Robot::getJointGroup(int jointID) const{
    if(_rootComponent != NULL){
        //Use a JointFinder to find the joint group
        //that contains the joint
        JointVisitor visitor;

        _rootComponent->accept(&visitor);
        return visitor.map()->value(jointID);
    }
    else{
        return NULL;
    }
}


//TODO: Refactor setJointAngle and moveJointBy for code reuse (function pointers?)
float Robot::setJointAngle(int jointID, float theta){
    float prevAngle, newAngle;
    RobotJointGroup* jointGroup;

    if((jointGroup = getJointGroup(jointID)) != NULL){
        prevAngle = jointGroup->jointAngle(jointID);
        newAngle = jointGroup->setJointAngle(jointID, theta);

        if(newAngle != prevAngle){
            _changed = true;

            //Emit the changed signals only if the angle has changed
            emit jointAngleChanged(jointID, newAngle, prevAngle);
            if(!_batchSem)
                //Only emit the robot changed signal if we aren't in
                //batch mode
                emit robotChanged();
        }

        return newAngle;
    }
    else{
        return INVALID_JOINT;
    }
}


float Robot::moveJointBy(int jointID, float delta){
    float prevAngle, newAngle;
    RobotJointGroup* jointGroup;

    if((jointGroup = getJointGroup(jointID)) != NULL){
        prevAngle = jointGroup->jointAngle(jointID);
        newAngle = jointGroup->moveJointBy(jointID, delta);

        if(newAngle != prevAngle){
            _changed = true;

            //Emit the changed signals only if the angle has changed
            emit jointAngleChanged(jointID, newAngle, prevAngle);
            if(!_batchSem)
                //Only emit the robot changed signal if we aren't in
                //batch mode
                emit robotChanged();
        }

        return newAngle;
    }
    else{
        return INVALID_JOINT;
    }
}

QSharedPointer<RobotPose> Robot::robotPose() const {
    QSharedPointer<RobotPose> robot(new RobotPose());
    foreach (int jointID, jointIDs()) {
        robot->setJointAngle(jointID, jointAngle(jointID));
    }
    return robot;
}


//TODO: Refactor so there is more code reuse between functions
//that access information about the robot? (Q_PROPERTY ?)
float Robot::jointAngle(int jointID) const{
    RobotJointGroup* jointGroup;

    if((jointGroup = getJointGroup(jointID)) != NULL)
        return jointGroup->jointAngle(jointID);
    else
        return INVALID_JOINT;
}

QString Robot::jointDescription(int jointID) const{
    RobotJointGroup* jointGroup = getJointGroup(jointID);

    if(jointGroup != NULL)
        return jointGroup->jointDescription(jointID);
    else
        return NULL;
}

float Robot::maximum(int jointID) const{
    RobotJointGroup* jointGroup = getJointGroup(jointID);

    if(jointGroup != NULL)
        return jointGroup->maximum(jointID);

    return INVALID_JOINT;
}

float Robot::minimum(int jointID) const{
    RobotJointGroup* jointGroup = getJointGroup(jointID);

    if(jointGroup != NULL)
        return jointGroup->minimum(jointID);

    return INVALID_JOINT;
}


void Robot::startBatch(){
    _batchSem++;
    _changed = false;
}

void Robot::endBatch(){
    if(!--_batchSem && _changed){
        //If we have a valid batch ending, and the robot actually
        //changed, emit a robotChanged signal
        emit robotChanged();
    }

    if(_batchSem < 0)
        _batchSem = 0;
}

bool Robot::batchMode(){
    return _batchSem > 0;
}


void Robot::accept(RobotVisitor *visitor){
    if(_rootComponent != NULL)
        _rootComponent->accept(visitor);
}



/*************************************************/
/*              JOINTVISITOR CLASS               */
/*************************************************/
void Robot::JointVisitor::visit(RobotComponent *component){
    int i;
    RobotJointGroup* group;
    QList<int> jointIDs;

    if(component->type() == JOINT_GROUP){
        //Query the joint group for the joints that it
        //contains
        group = (RobotJointGroup*)component;
        jointIDs = group->jointIDs();

        for(i = 0; i < jointIDs.length(); i++){
            _jointMap.insert(jointIDs.at(i), group);
        }
    }
}

QMap<int, RobotJointGroup*>* Robot::JointVisitor::map(){
    return &_jointMap;
}


/*************************************************/
/*            COMPONENTFINDER CLASS              */
/*************************************************/
Robot::ComponentFinder::ComponentFinder(const NameGroup componentNames) :
    _found(false),
    _component(NULL)
{
    int i;
    for(i = 0; i < MAX_NAMES; i++){
        _componentNames[i] = componentNames[i];
    }
}

void Robot::ComponentFinder::visit(RobotComponent *component){
    int i;
    bool equal = true;
    const unsigned int* names = component->names();

    //We already found it, just return
    if(_found)
        return;

    //Compare names to see if this is the segment we are looking for
    for(i = 0; i < MAX_NAMES; i++){
        if(names[i] != _componentNames[i]){
            equal = false;
            break;
        }
    }
    if(equal){
        _found = true;
        _component = component;
    }
}

RobotComponent* Robot::ComponentFinder::component(){
    return _component;
}


